#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H

#include <deque>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

#include "postprocess/common/common_func.h"

class KalmanFilter {
 public:
  explicit KalmanFilter();
  KalmanFilter(KalmanFilter const&) = delete;             // 禁止拷贝构造
  KalmanFilter& operator=(KalmanFilter const&) = delete;  // 禁止拷贝赋值
  KalmanFilter(KalmanFilter&&) = delete;                  // 禁止移动构造
  KalmanFilter& operator=(KalmanFilter&&) = delete;       // 禁止移动赋值
  ~KalmanFilter();

  /// @brief initialize the state of kalman filter
  /// @param anchor_point
  /// @param velocity
  void Initialize(Eigen::Vector3f const& anchor_point,
                  Eigen::Vector3f const& velocity);

  /// @brief update filter with object
  /// @param new_object
  /// @param old_object
  /// @param time_diff
  /// @param is_utm_flag
  void UpdateWithObject(TrackedObjectPtr& new_object,
                        TrackedObjectPtr const& old_object, double time_diff);

  /// @brief get state of filter with acceleration
  /// @param anchor_point
  /// @param velocity
  /// @param acceleration
  void GetState(Eigen::Vector3f* anchor_point, Eigen::Vector3f* velocity,
                Eigen::Vector3f* acceleration);

  /// @brief get online_covariance of filter
  /// @param[OUT] online_covariance: online covariance
  /// @return nothing
  void GetOnlineCovariance(Eigen::Matrix3f* online_covariance);

  /// @brief get acceleration uncertainity of filter
  /// @param acceleration_covariance
  void GetAccelerationCovariance(Eigen::Matrix3f* acceleration_covariance);

  /// @brief get acceleration gain of filter
  /// @param acceleration_gain
  void GetAccelerationGain(Eigen::Vector3f* acceleration_gain);

  /// @brief get velocity covariance of filter
  /// @param velocity_covariance
  void GetVelocityCovariance(Eigen::Matrix3f* velocity_covariance);

  /// @brief get state covariance
  void GetStateCovariance(Eigen::Matrix4f* state_covariance);

  /// @brief update filter without object
  /// @param time_diff
  void UpdateWithoutObject(double time_diff);

 private:
  /// @brief kalman filter update state with partial observation
  void update_state(TrackedObjectPtr const& new_object,
                    TrackedObjectPtr const& old_object, double time_diff);

  /// @brief evaluate online covariance
  void evaluate_online_covariance();

  /// @biref adjust gain based on the max allowed change of state
  void state_gain_adjustment(Eigen::Vector4f& gain);

  /// @brief compute update quality for adaptive filtering
  /// @param new_object
  /// @param old_object
  void compute_update_quality(TrackedObjectPtr const& new_object,
                              TrackedObjectPtr const& old_object);

  /// @brief compute update quality by using association score
  /// @param[IN] new_object: new object for current updating
  /// @return update quality according association score
  float compute_update_quality_according_association_score(
      TrackedObjectPtr const& new_object);

  /// @brief compute velocity based on motion consistency
  Eigen::VectorXf compute_velocity(TrackedObjectPtr const& new_object,
                                   TrackedObjectPtr const& old_object,
                                   double time_diff) noexcept;

  /// @brief compute measured bbox center velocity
  /// @param new_object
  /// @param old_object
  /// @param time_diff
  Eigen::VectorXf compute_measured_center_velocity(
      TrackedObjectPtr const& new_object, TrackedObjectPtr const& old_object,
      double time_diff) noexcept;

  /// @brief compute measured bbox concor velocity
  /// @param new_object
  /// @param old_object
  /// @param time_diff
  Eigen::VectorXf compute_measured_corner_velocity(
      TrackedObjectPtr const& new_object, TrackedObjectPtr const& old_object,
      double time_diff) noexcept;

  /// @brief  reverse array
  /// @param arr
  /// @param begin
  /// @param end
  void ReverseArray(int arr[], int begin, int end) noexcept;

  /// @brief adaptive  目前我们的配置中使用adaptive  20220606
  static bool s_use_adaptive_;
  /// @brief is livox
  static bool use_ssld_;
  /// @brief use breakdown
  static bool s_use_breakdown_;
  /// @brief
  static float s_association_score_maximum_;
  /// @brief parameters
  static Eigen::Matrix3f s_propagation_noise_;
  /// @brief
  static float s_measurement_noise_;
  /// @brief init velocity noise
  static float s_initial_velocity_noise_;
  /// @brief init acceleration noise
  static float s_initial_acceleration_noise_;
  /// @brief
  static float s_breakdown_threshold_maximum_;
  /// @brief predit variance per sqr second
  static float s_predict_variance_per_sqrsec_;
  /// @brief
  static std::uint32_t s_measurement_cached_history_size_minimum_;
  /// @brief
  static std::uint32_t s_measurement_cached_history_size_maximum_;
  /// @brief
  std::uint32_t measurement_cached_history_size_;
  /// @brief filter history
  std::int32_t age_;
  /// @brief history velocity and timediff
  std::deque<Eigen::Vector3f> history_measured_velocity_;
  /// @brief
  std::deque<double> history_time_diff_;
  /// @brief velocity covariances
  Eigen::Matrix3f velocity_covariance_;
  /// @brief acceleration covariances
  Eigen::Matrix3f acceleration_covariance_;
  /// @brief online velocity covariance
  Eigen::Matrix3f online_velocity_covariance_;
  /// @brief anchor point
  Eigen::Vector3f belief_center_point_;
  /// @brief belief velocity
  Eigen::Vector3f belief_velocity_;
  /// @brief belief acceleration
  Eigen::Vector3f belief_acceleration_;
  /// @brief belief acceleration gain
  Eigen::Vector3f belief_acceleration_gain_;
  /// @brief selected measurement velocity
  Eigen::Vector3f select_measured_velocity_;
  /// @brief selected measurement acceleration
  Eigen::Vector3f select_measured_acceleration_;
  /// @brief state combine velocity and acceleration
  Eigen::Vector4f state_;
  /// @brief measurement covariance for state
  Eigen::Matrix4f measurement_covariance_;
  /// @brief state covariance
  Eigen::Matrix4f state_covariance_;
  /// @brief update quality
  float update_quality_;
  /// @brief breakdown threshold
  float breakdown_threshold_;
};

#endif
